﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;

namespace FrogEngine
{
	public static class CollisionHelper
	{
		#region Triangle-Triangle Intersection

		public static bool TriangleTriangleIntersection(Triangle t1, Triangle t2)
		{
			Vector3 E1, E2, N, N1, N2, tmp, VV0 = Vector3.Zero, VV1 = Vector3.Zero, VV2 = Vector3.Zero, UU0 = Vector3.Zero, UU1 = Vector3.Zero, UU2 = Vector3.Zero;
			float d1, d2, du0, du1, du2, dv0, dv1, dv2, tdu0 = 0, tdu1 = 0, tdv0 = 0, tdv1 = 0, du01, du02;

			E1 = t2.V2 - t2.V1;
			E2 = t2.V3 - t2.V1;
			N1 = Vector3.Cross(E1, E2);
			d1 = -Vector3.Dot(N1, t2.V1);

			dv0 = Vector3.Dot(N1, t1.V1) + d1;
			dv1 = Vector3.Dot(N1, t1.V2) + d1;
			dv2 = Vector3.Dot(N1, t1.V3) + d1;

			if (Math.Abs(dv0) < PrecisionHelper.EPSILON) dv0 = 0f;
			if (Math.Abs(dv1) < PrecisionHelper.EPSILON) dv1 = 0f;
			if (Math.Abs(dv2) < PrecisionHelper.EPSILON) dv2 = 0f;

			if (SameSign(dv0, dv1, dv2))
				return false;

			E1 = t1.V2 - t1.V1;
			E2 = t1.V3 - t1.V1;
			N2 = Vector3.Cross(E1, E2);
			d2 = -Vector3.Dot(N2, t1.V1);

			du0 = Vector3.Dot(N2, t2.V1) + d2;
			du1 = Vector3.Dot(N2, t2.V2) + d2;
			du2 = Vector3.Dot(N2, t2.V3) + d2;

			if (Math.Abs(du0) < PrecisionHelper.EPSILON) du0 = 0f;
			if (Math.Abs(du1) < PrecisionHelper.EPSILON) du1 = 0f;
			if (Math.Abs(du2) < PrecisionHelper.EPSILON) du2 = 0f;

			if (SameSign(du0, du1, du2))
				return false;

			if ((dv0 == 0) && (dv1 == 0) && (dv2 == 0))
				return CoplanarTriangleIntersection(N1, t1, t2);

			if (SignDifSign(dv0, dv1, dv2))
			{
				VV0 = t1.V1; VV1 = t1.V2; VV2 = t1.V3; tdv0 = dv0; tdv1 = dv1;
			}
			else if (SignDifSign(dv2, dv0, dv1))
			{
				VV0 = t1.V2; VV1 = t1.V3; VV2 = t1.V1; tdv0 = dv1; tdv1 = dv2;
			}
			else if (SignDifSign(dv0, dv1, dv2))
			{
				VV0 = t1.V3; VV1 = t1.V1; VV2 = t1.V2; tdv0 = dv2; tdv1 = dv0;
			}

			if (SignDifSign(du1, du2, du0))
			{
				UU0 = t2.V1; UU1 = t2.V2; UU2 = t2.V3; tdu0 = du0; tdu1 = du1;
			}
			else if (SignDifSign(du2, du0, du1))
			{
				UU0 = t2.V2; UU1 = t2.V3; UU2 = t2.V1; tdu0 = du1; tdu1 = du2; 
			}
			else if (SignDifSign(du0, du1, du2))
			{
				UU0 = t2.V3; UU1 = t2.V1; UU2 = t2.V2; tdu0 = du2; tdu1 = du0;
			}

			if ((tdu0 >= 0) && (tdu1 <= 0))
			{
				if ((tdv0 >= 0) && (tdv1 <= 0))
				{
					E1 = VV0 - VV2;
					E2 = UU0 - UU2;
					N = Vector3.Cross(E1, E2);
					tmp = UU0 - VV0;
					du02 = Vector3.Dot(N, tmp);
					if (du02 > PrecisionHelper.EPSILON)
						return false;

					E1 = VV1 - VV0;
					E2 = UU1 - UU0;
					N = Vector3.Cross(E1, E2);
					tmp = UU0 - VV0;
					du01 = Vector3.Dot(N, tmp);
					if (du01 < -PrecisionHelper.EPSILON)
						return false;
				}
				else
				{
					E1 = VV1 - VV0;
					E2 = UU0 - UU2;
					N = Vector3.Cross(E1, E2);
					tmp = UU0 - VV0;
					du02 = Vector3.Dot(N, tmp);
					if (du02 > PrecisionHelper.EPSILON)
						return false;

					E1 = VV0 - VV2;
					E2 = UU1 - UU0;
					N = Vector3.Cross(E1, E2);
					tmp = UU0 - VV0;
					du01 = Vector3.Dot(N, tmp);
					if (du01 < -PrecisionHelper.EPSILON)
						return false;
				}
			}
			else
			{
				if ((tdv0 >= 0) && (tdv1 <= 0))
				{
					E1 = VV0 - VV2;
					E2 = UU1 - UU0;
					N = Vector3.Cross(E1, E2);
					tmp = UU0 - VV0;
					du01 = Vector3.Dot(N, tmp);
					if (du01 > PrecisionHelper.EPSILON)
						return false;

					E1 = VV1 - VV0;
					E2 = UU0 - UU2;
					N = Vector3.Cross(E1, E2);
					tmp = UU0 - VV0;
					du02 = Vector3.Dot(N, tmp);
					if (du02 < -PrecisionHelper.EPSILON)
						return false;
				}
				else
				{
					E1 = VV1 - VV0;
					E2 = UU1 - UU0;
					N = Vector3.Cross(E1, E2);
					tmp = UU0 - VV0;
					du01 = Vector3.Dot(N, tmp);
					if (du01 > PrecisionHelper.EPSILON)
						return false;

					E1 = VV0 - VV2;
					E2 = UU0 - UU2;
					N = Vector3.Cross(E1, E2);
					tmp = UU0 - VV0;
					du02 = Vector3.Dot(N, tmp);
					if (du02 < -PrecisionHelper.EPSILON)
						return false;
				}
			}

			return true;
		}

		private static bool SameSign(float v1, float v2, float v3)
		{
			return (v1 * v2 > 0) && (v1 * v3 > 0);
		}

		private static bool SignDifSign(float d0, float d1, float d2)
		{
			return ((d0 >= 0) && (d1 >= 0) && (d2 < 0) ||
					(d0 <= 0) && (d1 <= 0) && (d2 > 0) ||
					(d0 > 0) && (d1 > 0) && (d2 <= 0) ||
					(d0 < 0) && (d1 < 0) && (d2 >= 0));
		}

		private static bool CoplanarTriangleIntersection(Vector3 N, Triangle t1, Triangle t2)
		{
			Vector3 A;
			short i0, i1;
			A.X = Math.Abs(N.X);
			A.Y = Math.Abs(N.Y);
			A.Z = Math.Abs(N.Z);

			if (A.X > A.Y)
			{
				if (A.X > A.Z)
				{
					i0 = 1;
					i1 = 2;
				}
				else
				{
					i0 = 0;
					i1 = 1;
				}
			}
			else
			{
				if (A.Z > A.Y)
				{
					i0 = 0;
					i1 = 1;
				}
				else
				{
					i0 = 0;
					i1 = 2;
				}
			}

			if (EdgeAgainstTriangleEdges(t1.V1, t1.V2, t2.V1, t2.V2, t2.V3, i0, i1))
				return true;
			if (EdgeAgainstTriangleEdges(t1.V2, t1.V3, t2.V1, t2.V2, t2.V3, i0, i1))
				return true;
			if (EdgeAgainstTriangleEdges(t1.V3, t1.V1, t2.V1, t2.V2, t2.V3, i0, i1))
				return true;

			if (PointInTriangle(t1.V1, t2.V1, t2.V2, t2.V3, i0, i1))
				return true;
			if (PointInTriangle(t2.V1, t1.V1, t1.V2, t1.V3, i0, i1))
				return true;

			return false;
		}

		private static bool EdgeAgainstTriangleEdges(Vector3 V0, Vector3 V1, Vector3 U0, Vector3 U1, Vector3 U2, short i0, short i1)
		{
			float Ax, Ay;
			Ax = V1.X - V0.X;
			Ay = V1.Y - V0.Y;

			return EdgeEdgeTest(V0, U0, U1, Ax, Ay, i0, i1) ||
					EdgeEdgeTest(V0, U1, U2, Ax, Ay, i0, i1) ||
					EdgeEdgeTest(V0, U2, U0, Ax, Ay, i0, i1);
		}

		public static float ElementAt(this Vector3 v, short i)
		{
			if (i == 0)
				return v.X;
			else if (i == 1)
				return v.Y;
			else if (i == 2)
				return v.Z;
			else
				return 0f;
		}

		private static bool EdgeEdgeTest(Vector3 V0, Vector3 U0, Vector3 U1, float Ax, float Ay, short i0, short i1)
		{
			float Bx = U0.ElementAt(i0) - U1.ElementAt(i0);
			float By = U0.ElementAt(i1) - U1.ElementAt(i1);
			float Cx = V0.ElementAt(i0) - U0.ElementAt(i0);
			float Cy = V0.ElementAt(i1) - U0.ElementAt(i1);
			float f = Ay * Bx - Ax * By;
			float d = By * Cx - Bx * Cy;

			if ((f > 0 && d >= 0 && d <= f) || (f < 0 && d <= 0 && d >= f))
			{
				float e = Ax * Cy - Ay * Cx;
				if (f > 0)
				{
					if (e >= 0 && e <= f)
						return true;
					else
						return false;
				}
				else
				{
					if (e <= 0 && e >= f)
						return true;
					else
						return false;
				}
			}
			else
				return false;
		}

		private static bool PointInTriangle(Vector3 V0, Vector3 U0, Vector3 U1, Vector3 U2, short i0, short i1)
		{
			float a, b, c, d0, d1, d2;
			a = U1.ElementAt(i1) - U0.ElementAt(i1);
			b = -(U1.ElementAt(i0) - U0.ElementAt(i0));
			c = -a * U0.ElementAt(i0) - b * U0.ElementAt(i1);
			d0 = a * V0.ElementAt(i0) + b * V0.ElementAt(i1) + c;

			a = U2.ElementAt(i1) - U1.ElementAt(i1);
			b = -(U2.ElementAt(i0) - U1.ElementAt(i0));
			c = -a * U1.ElementAt(i0) - b * U1.ElementAt(i1);
			d1 = a * V0.ElementAt(i0) + b * V0.ElementAt(i1) + c;

			a = U0.ElementAt(i1) - U2.ElementAt(i1);
			b = -(U0.ElementAt(i0) - U2.ElementAt(i0));
			c = -a * U2.ElementAt(i0) - b * U2.ElementAt(i1);
			d2 = a * V0.ElementAt(i0) + b * V0.ElementAt(i1) + c;

			if (d0 * d1 > 0)
			{
				if (d0 * d2 > 0)
					return true;
				else
					return false;
			}
			else
				return false;
		}

		#endregion

		/// <summary>
		/// Checks whether a ray intersects a triangle. This uses the algorithm
		/// developed by Tomas Moller and Ben Trumbore, which was published in the
		/// Journal of Graphics Tools, volume 2, "Fast, Minimum Storage Ray-Triangle
		/// Intersection".
		/// 
		/// This method is implemented using the pass-by-reference versions of the
		/// XNA math functions. Using these overloads is generally not recommended,
		/// because they make the code less readable than the normal pass-by-value
		/// versions. This method can be called very frequently in a tight inner loop,
		/// however, so in this particular case the performance benefits from passing
		/// everything by reference outweigh the loss of readability.
		/// </summary>
		public static void RayIntersectsTriangle(ref Ray ray,
										  ref Vector3 vertex1,
										  ref Vector3 vertex2,
										  ref Vector3 vertex3, out float? result)
		{
			// Compute vectors along two edges of the triangle.
			Vector3 edge1, edge2;

			Vector3.Subtract(ref vertex2, ref vertex1, out edge1);
			Vector3.Subtract(ref vertex3, ref vertex1, out edge2);

			// Compute the determinant.
			Vector3 directionCrossEdge2;
			Vector3.Cross(ref ray.Direction, ref edge2, out directionCrossEdge2);

			float determinant;
			Vector3.Dot(ref edge1, ref directionCrossEdge2, out determinant);

			// If the ray is parallel to the triangle plane, there is no collision.
			if (determinant > -float.Epsilon && determinant < float.Epsilon)
			{
				result = null;
				return;
			}

			float inverseDeterminant = 1.0f / determinant;

			// Calculate the U parameter of the intersection point.
			Vector3 distanceVector;
			Vector3.Subtract(ref ray.Position, ref vertex1, out distanceVector);

			float triangleU;
			Vector3.Dot(ref distanceVector, ref directionCrossEdge2, out triangleU);
			triangleU *= inverseDeterminant;

			// Make sure it is inside the triangle.
			if (triangleU < 0 || triangleU > 1)
			{
				result = null;
				return;
			}

			// Calculate the V parameter of the intersection point.
			Vector3 distanceCrossEdge1;
			Vector3.Cross(ref distanceVector, ref edge1, out distanceCrossEdge1);

			float triangleV;
			Vector3.Dot(ref ray.Direction, ref distanceCrossEdge1, out triangleV);
			triangleV *= inverseDeterminant;

			// Make sure it is inside the triangle.
			if (triangleV < 0 || triangleU + triangleV > 1)
			{
				result = null;
				return;
			}

			// Compute the distance along the ray to the triangle.
			float rayDistance;
			Vector3.Dot(ref edge2, ref distanceCrossEdge1, out rayDistance);
			rayDistance *= inverseDeterminant;

			// Is the triangle behind the ray origin?
			if (rayDistance < 0)
			{
				result = null;
				return;
			}

			result = rayDistance;
		}
	}
}
